#include "ros/ros.h"
#include "auto_msgs/auto_ctr.h"
#include "yaml-cpp/yaml.h"
#include "dashboard_config_path.h"
#include "keyboard_reader/Key.h"

keyboard_reader::Key key_msg;

void cb_keyboard(const keyboard_reader::Key &msg)
{
	key_msg = msg;
	std::cout << msg.key_name << std::endl;
}
int main(int argc, char **argv)
{
	key_msg.key_name = "S";
	ros::init(argc, argv, "dashboard");
	ros::NodeHandle n;
	//topic的名称为chatter,1000为缓冲区，缓冲区中的消息在大于 1000 个的时候就会开始丢弃先前发布的消息。
	ros::Publisher auto_pub = n.advertise<auto_msgs::auto_ctr>("auto_msg", 5);
	auto_msgs::auto_ctr auto_msg;
	auto_msg.ctr_mode = 1; //wait by
	double rate = 50;
	ros::Rate loop_rate(rate); //发布频率为10FPS
	double time_elapse = 0;
	ros::Subscriber localizition_sub_ =
		n.subscribe("keyboard", 3, cb_keyboard);
	std::string auto_ctr_yaml_file_name = dashboard_config_path + "auto_ctr.yaml";
	YAML::Node auto_ctr_yaml = YAML::LoadFile(auto_ctr_yaml_file_name);
	// rate = node_yaml["loop_rate"].as<double>();
	double trans_time = auto_ctr_yaml["trans_time"].as<double>();

	while (1)
	{
		if (key_msg.key_name == "w" || key_msg.key_name == "W")
		{

			auto_msg.ctr_mode = 2; //1; //auto
			std::cout << "ctr_mode is auto ctr" << std::endl;
		}
		else
		{
			time_elapse += 1 / rate;
			auto_msg.ctr_mode = 1; //stand_by
								   //std::cout << "ctr_mode is stand_by" << std::endl;
		}

		auto_pub.publish(auto_msg);
		ros::spinOnce(); //可用回调函数
		loop_rate.sleep();
	}

	return 0;
}